import painting.GUI;
import painting.Painter;

public class MappingRobot extends Thread {
	remoteApi vrep;
	int clientID;
	int leftMotor;
	int rightMotor;
	int frontRight;
	int sideRight;
	int frontLeft;
	int sideLeft;
	private Painter painter;
	private int dr20;
	boolean test = true;
	float xPos = (float) -0.75;
	float yPos = (float) -0.25;
	int xCord = 6; //4
	int yCord = 4; //4
	int xOldCord = 6;
	int yOldCord = 4;
	
	
	public static final  int UP = 1;
	public static final int RIGHT = 2;
	public static final int DOWN = 3;
	public static final int LEFT = 4;
	int facing = LEFT;
	private GUI map;
	public static int SleepCount = 1;
	
	public MappingRobot(Painter painter, GUI map) {
		this.painter = painter;
		this.map = map;
	}
	
	public void run() {
		 vrep = new remoteApi();
		 clientID = vrep.simxStart("127.0.0.1", 21000, true, true, 5000, 5);
		if (clientID != -1) {
			System.out.println("Connected to remote API server");
			IntW handle = new IntW(-1);
			int a = vrep.simxGetObjectHandle(clientID, "dr20_leftWheelJoint_",
					handle, vrep.simx_opmode_oneshot_wait);
			 leftMotor = handle.w;
			if (a == vrep.simx_error_noerror) {
				System.out.println(leftMotor);
			}
			handle.w = -1;
			int b = vrep.simxGetObjectHandle(clientID, "dr20_rightWheelJoint_",
					handle, vrep.simx_opmode_oneshot_wait);
			 rightMotor = handle.w;
			handle.w = -1;
			int c = vrep.simxGetObjectHandle(clientID, "dr20_infraredSensor1_",
					handle, vrep.simx_opmode_oneshot_wait);
			 frontRight = handle.w;
			handle.w = -1;
			int d = vrep.simxGetObjectHandle(clientID, "dr20_infraredSensor2_",
					handle, vrep.simx_opmode_oneshot_wait);
			 sideRight = handle.w;
			handle.w = -1;
			int e = vrep.simxGetObjectHandle(clientID, "dr20_infraredSensor6_",
					handle, vrep.simx_opmode_oneshot_wait);
			 frontLeft = handle.w;
			handle.w = -1;
			int f = vrep.simxGetObjectHandle(clientID, "dr20_infraredSensor5_",
					handle, vrep.simx_opmode_oneshot_wait);
			 sideLeft = handle.w;
			 
			 handle.w = -1;
				int g = vrep.simxGetObjectHandle(clientID, "dr20",
						handle, vrep.simx_opmode_oneshot_wait);
				 dr20 = handle.w;
				 
				 handle.w = -1;
					int h = vrep.simxGetObjectHandle(clientID, "dr20_ultrasonicSensor_",
							handle, vrep.simx_opmode_oneshot_wait);
					 int front = handle.w;
			 
			 
			// simxGetVisionSensorDepthBuffer  Need to look a bit more on this one.



	

			System.out.println(c + " " + b + " " + c + " " + d + " " + e + " "
					+ f);

			map.updateRobotPos(xCord,yCord,xOldCord,yOldCord);
			int count = 0;
			while (vrep.simxGetConnectionId(clientID) != -1) {

				
				
				FloatWA sideRightPosition = new FloatWA(3);
				vrep.simxGetObjectPosition(clientID,dr20, -1, sideRightPosition, vrep.simx_opmode_streaming);
				try {
					sleep(50);
				} catch (InterruptedException e1) {
					// TODO Auto-generated catch block
					e1.printStackTrace();
				}
				vrep.simxGetObjectPosition(clientID,dr20, -1, sideRightPosition, vrep.simx_opmode_streaming);
				if(test){
					//turnLeft();
				//System.out.println("position: " + sideRightPosition.w[0] + " " + sideRightPosition.w[1] + " " + sideRightPosition.w[2]);
				}
				//if((int)(Math.random() * ((2 - 1) + 1)) == 1){
					//turnRight();
				//}else{
					//turnLeft();
				//}
				detect(front);
				map.update();
				boolean canMoveForward = true;
				int[] temp = map.findNextStep();
				
				//
				//map.update();
				System.out.println("Coordinats to move to: " + temp[0] + " " + temp[1]);
				System.out.println("Current Coordinates: " + xCord + " " + yCord);
				if(temp[0] == xCord + 1 && map.canMoveForward(xCord + 1,yCord) && count !=4){
					switch(facing){
					case UP: turnRight(); detect(front);map.update();forward(sideRightPosition);break;
					case DOWN: turnLeft(); detect(front);map.update();forward(sideRightPosition);break;
					case LEFT: turnLeft(); detect(front);map.update();turnLeft(); detect(front);map.update();forward(sideRightPosition);break;
					case RIGHT: forward(sideRightPosition);break;
					}
				}else if(temp[0] == xCord - 1 && map.canMoveForward(xCord - 1,yCord)&& count !=4){
					switch(facing){
					case UP: turnLeft(); detect(front);map.update();forward(sideRightPosition);break;
					case DOWN: turnRight(); detect(front);map.update();forward(sideRightPosition);break;
					case LEFT: forward(sideRightPosition);break;
					case RIGHT: turnRight(); detect(front);map.update();turnRight(); detect(front);map.update();forward(sideRightPosition);break;
					}
					
			    }else if(temp[1] == yCord + 1 && map.canMoveForward(xCord ,yCord+1)&& count !=4){
			    	switch(facing){
					case UP: turnLeft(); detect(front);map.update();turnLeft(); detect(front);map.update();forward(sideRightPosition);break;
					case DOWN: forward(sideRightPosition);break;
					case LEFT:  turnLeft(); detect(front);map.update();forward(sideRightPosition);break;
					case RIGHT: turnRight(); detect(front);map.update();forward(sideRightPosition);break;
					}
			    }else if( temp[1] == yCord -1 && map.canMoveForward(xCord,yCord-1)&& count !=4){
			    	switch(facing){
					case UP: forward(sideRightPosition);break;
					case DOWN: turnRight(); detect(front);map.update();turnRight(); detect(front);map.update();forward(sideRightPosition);break;
					case LEFT:  turnRight(); detect(front);map.update();forward(sideRightPosition);break;
					case RIGHT: turnLeft(); detect(front);map.update();forward(sideRightPosition);break;
					}
			    }else{
			    	System.out.println("RANDOM!!!!!");
			    	//RANDOM!
	
					if((int)(Math.random() * ((2 - 1) + 1)) == 1){
						turnRight();
					}else{
						turnLeft();
					}
					detect(front);
					map.update();
					switch(facing){
					case UP: canMoveForward = map.canMoveForward(xCord,yCord -1);break;
					case DOWN: canMoveForward = map.canMoveForward(xCord,yCord +1);break;
					case LEFT: canMoveForward = map.canMoveForward(xCord-1,yCord);break;
					case RIGHT: canMoveForward = map.canMoveForward(xCord+1,yCord);break;
				}
				while(!canMoveForward){
					turnRight();
					switch(facing){
						case UP: canMoveForward = map.canMoveForward(xCord,yCord -1);break;
						case DOWN: canMoveForward = map.canMoveForward(xCord,yCord +1);break;
						case LEFT: canMoveForward = map.canMoveForward(xCord-1,yCord);break;
						case RIGHT: canMoveForward = map.canMoveForward(xCord+1,yCord);break;
					}
					detect(front);
					map.update();
				}
				forward(sideRightPosition);
				detect(front);
				map.update();
			    }
				
				

//				
				map.update();
				detect(front);
				map.updateRobotPos(xCord,yCord,xOldCord,yOldCord);
				System.out.println("Updating");
				map.update();

				
				
				FloatWA angles = new FloatWA(3);
				vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
				try {
					sleep(50);
				} catch (InterruptedException e1) {
					// TODO Auto-generated catch block
					e1.printStackTrace();
				}
				//System.out.println("Angle: " + angles.w[0] + " " + angles.w[1] + " " + angles.w[2]);
				vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
				//System.out.println("position: " + angles.w[0] + " " + angles.w[1] + " " + angles.w[2]);
				

				
				
				
				
				//forward(sideRightPosition);
				int leftCount = 0;
				int rightCount = 0;
				int xPixels = Math.round(-sideRightPosition.w[0]*100);
				int yPixels = Math.round(sideRightPosition.w[1]*100);
				//painter.addBlackSquare(500 + xPixels, 500 + yPixels);
				//for(int i = 1; i < 50; i++){
				//detect(front);
				

				
				
//				if(bw.w == false && bw2.w == false && bw3.w == false && bw4.w == false && bw5.w == true){
//					
//					
//				}




				count++;
				if(count == 5){
					count = 0;
				}
			}

		}
	}

	private void detect(int front) {
		BoolW bw = new BoolW(false);
		FloatWA detectionsPoint = new FloatWA(3);
		vrep.simxReadProximitySensor(clientID, frontRight, bw, detectionsPoint, null, null, vrep.simx_opmode_streaming);
		BoolW bw2 = new BoolW(false);
		FloatWA detectionsPoint2 = new FloatWA(3);
		vrep.simxReadProximitySensor(clientID, sideRight, bw2, detectionsPoint2, null, null, vrep.simx_opmode_streaming);
		BoolW bw3 = new BoolW(false);
		FloatWA detectionsPoint3 = new FloatWA(3);
		vrep.simxReadProximitySensor(clientID, frontLeft, bw3, detectionsPoint3, null, null, vrep.simx_opmode_streaming);
		BoolW bw4 = new BoolW(false);
		FloatWA detectionsPoint4 = new FloatWA(3);
		vrep.simxReadProximitySensor(clientID, sideLeft, bw4, detectionsPoint4, null, null, vrep.simx_opmode_streaming);
		
		BoolW bw5 = new BoolW(false);
		FloatWA detectionsPoint5 = new FloatWA(3);
		vrep.simxReadProximitySensor(clientID, front, bw5, detectionsPoint5, null, null, vrep.simx_opmode_streaming);
		
		try {
			sleep(50);
		} catch (InterruptedException e1) {
			// TODO Auto-generated catch block
			e1.printStackTrace();
		}
		
		
		
		
		 bw = new BoolW(false);
		 detectionsPoint = new FloatWA(3);
		vrep.simxReadProximitySensor(clientID, frontRight, bw, detectionsPoint, null, null, vrep.simx_opmode_streaming);
		 bw2 = new BoolW(false);
		 detectionsPoint2 = new FloatWA(3);
		vrep.simxReadProximitySensor(clientID, sideRight, bw2, detectionsPoint2, null, null, vrep.simx_opmode_streaming);
		 bw3 = new BoolW(false);
		 detectionsPoint3 = new FloatWA(3);
		vrep.simxReadProximitySensor(clientID, frontLeft, bw3, detectionsPoint3, null, null, vrep.simx_opmode_streaming);
		 bw4 = new BoolW(false);
		 detectionsPoint4 = new FloatWA(3);
		vrep.simxReadProximitySensor(clientID, sideLeft, bw4, detectionsPoint4, null, null, vrep.simx_opmode_streaming);
		
		 bw5 = new BoolW(false);
		 detectionsPoint5 = new FloatWA(3);
		vrep.simxReadProximitySensor(clientID, front, bw5, detectionsPoint5, null, null, vrep.simx_opmode_streaming);
		
		
		
		
		if (bw.w == true && detectionsPoint.w[2] > 0.24 ) {
		//	int xdetect = Math.round(-sideRightPosition.w[0]*100 + -detectionsPoint.w[0]*1000);
			//int ydetect = Math.round(sideRightPosition.w[1]*100 + detectionsPoint.w[1]*1000);
			//painter.addRedSquare(500 + xdetect, 500 + ydetect);
			
			//System.out.println(detectionsPoint.w[0] + " " + detectionsPoint.w[1] + " " + detectionsPoint.w[2]);
			//if( detectionsPoint.w[2] < (float)0.4){
				//System.out.println(detectionsPoint.w[0] + " " + detectionsPoint.w[1] + " " + detectionsPoint.w[2]);
				//rightCount++;
			//}
			
			
			int length = 1;
			switch(facing){
				case UP: map.addObstacle(xCord + length,yCord -length);break;
				case DOWN: map.addObstacle(xCord - length,yCord + length);break;
				case LEFT: map.addObstacle(xCord-length,yCord-length);break;
				case RIGHT: map.addObstacle(xCord + length,yCord+length);break;
			}
		}else{
			int length = 1;
			switch(facing){
				case UP: map.addFreeSpace(xCord + length,yCord -length);break;
				case DOWN: map.addFreeSpace(xCord - length,yCord + length);break;
				case LEFT: map.addFreeSpace(xCord-length,yCord-length);break;
				case RIGHT: map.addFreeSpace(xCord + length,yCord+length);break;
		}
		}
			

			
		
		if(bw2.w == true && (detectionsPoint2.w[2] > 0.66 || detectionsPoint2.w[2] < 0.5)){
			//System.out.println(detectionsPoint2.w[0] + " " + detectionsPoint2.w[1] + " " + detectionsPoint2.w[2]);
			//int xdetect = Math.round(-sideRightPosition.w[0]*100 + -detectionsPoint2.w[0]*1000);
			//int ydetect = Math.round(sideRightPosition.w[1]*100 + detectionsPoint2.w[1]*1000);
			//painter.addRedSquare(500 + xdetect, 500 + ydetect);
			
			//System.out.println(detectionsPoint2.w[0] + " " + detectionsPoint2.w[1] + " " + detectionsPoint2.w[2]);
//					if( detectionsPoint2.w[2] < (float)0.3){  // Only count if we are close! No need to if we still are far away from the object! Need to do the same for the others, works a bit differently.
//						//rightCount++;
//						
//					}
			int length = 1;
			if( detectionsPoint2.w[2] > 0.66){//66
				length = 2;	
			}

			
			
			switch(facing){
			case UP:	map.addObstacle(xCord + length,yCord);
						if(length == 2){
							map.addFreeSpace(xCord + length-1,yCord);
						}
						break;
			case DOWN:	map.addObstacle(xCord - length,yCord);
						if(length == 2){
							map.addFreeSpace(xCord - length+1,yCord);
						}
			
						break;
			case LEFT: 	map.addObstacle(xCord,yCord-length);
						if(length == 2){
							map.addFreeSpace(xCord,yCord-length+1);
						}
						break;
			case RIGHT: map.addObstacle(xCord,yCord+length);
						if(length == 2){
							map.addFreeSpace(xCord,yCord+length-1);
						}
						break;
			}	
			
			
		}else{
			
			switch(facing){
			case UP:	map.addFreeSpace(xCord + 1,yCord);
						map.addFreeSpace(xCord + 2,yCord);
						break;
			case DOWN:	map.addFreeSpace(xCord - 1,yCord);
						map.addFreeSpace(xCord - 2,yCord);			
						break;
			case LEFT: 	map.addFreeSpace(xCord,yCord-1);;
						map.addFreeSpace(xCord,yCord-2);
						break;
			case RIGHT: map.addFreeSpace(xCord,yCord+1);
						map.addFreeSpace(xCord,yCord+2);
						break;
			}	
			
			
			
			
		}
		 if(bw3.w == true && detectionsPoint3.w[2] > 0.24){
				//int xdetect = Math.round(-sideRightPosition.w[0]*100 + -detectionsPoint3.w[0]*1000);
				//int ydetect = Math.round(sideRightPosition.w[1]*100 + detectionsPoint3.w[1]*1000);
				//painter.addRedSquare(500 + xdetect, 500 + ydetect);
			// System.out.println(detectionsPoint3.w[0] + " " + detectionsPoint3.w[1] + " " + detectionsPoint3.w[2]);
			
			 int length = 1;
				switch(facing){
					case UP: map.addObstacle(xCord - length,yCord -length);break;
					case DOWN: map.addObstacle(xCord + length,yCord + length);break;
					case LEFT: map.addObstacle(xCord-length,yCord+length);break;
					case RIGHT: map.addObstacle(xCord + length,yCord-length);break;
				}
				

		}else{
			 int length = 1;
				switch(facing){
					case UP: map.addFreeSpace(xCord - length,yCord -length);break;
					case DOWN: map.addFreeSpace(xCord + length,yCord + length);break;
					case LEFT: map.addFreeSpace(xCord-length,yCord+length);break;
					case RIGHT: map.addFreeSpace(xCord + length,yCord-length);break;
				}
		}
		if(bw4.w == true  && (detectionsPoint4.w[2] > 0.66 || detectionsPoint4.w[2] < 0.5)){
			//System.out.println(detectionsPoint4.w[0] + " " + detectionsPoint4.w[1] + " " + detectionsPoint4.w[2]);
			
			//int xdetect = Math.round(-sideRightPosition.w[0]*100 + -detectionsPoint4.w[0]*1000);
			//int ydetect = Math.round(sideRightPosition.w[1]*100 + detectionsPoint4.w[1]*1000);
			//painter.addRedSquare(500 + xdetect, 500 + ydetect);
			int length = 1;
			if( detectionsPoint4.w[2] > 0.66){
				length = 2;	
			}
			switch(facing){
			case UP:	map.addObstacle(xCord - length,yCord);
						if(length == 2){
							map.addFreeSpace(xCord - length+1,yCord);
						}
						break;
			case DOWN:	map.addObstacle(xCord + length,yCord);
						if(length == 2){
							map.addFreeSpace(xCord + length-1,yCord);
						}
			
						break;
			case LEFT: 	map.addObstacle(xCord,yCord+length);
						if(length == 2){
							map.addFreeSpace(xCord,yCord+length-1);
						}
						break;
			case RIGHT: map.addObstacle(xCord,yCord-length);
						if(length == 2){
							map.addFreeSpace(xCord,yCord-length+1);
						}
						break;
			}	
			
			

		}else{
			switch(facing){
			case UP:	map.addFreeSpace(xCord - 1,yCord);
						map.addFreeSpace(xCord - 2,yCord);
						break;	
			case DOWN:	map.addFreeSpace(xCord + 2,yCord);
						map.addFreeSpace(xCord + 1,yCord);
						break;
			case LEFT: 	map.addFreeSpace(xCord,yCord+1);
						map.addFreeSpace(xCord,yCord+2);
						break;
			case RIGHT: map.addFreeSpace(xCord,yCord-1);
						map.addFreeSpace(xCord,yCord-2);	
						break;
			}	
		}
		
		
		if(bw5.w == true){
//			System.out.println(detectionsPoint5.w[0] + " " + detectionsPoint5.w[1] + " " + detectionsPoint5.w[2]);
//			Float fl = new Float(detectionsPoint5.w[0]);
//	        Double dob = new Double(fl.toString());
//			System.out.println(Math.toDegrees(dob));
//			 fl = new Float(detectionsPoint5.w[1]);
//	         dob = new Double(fl.toString());
//			System.out.println(Math.toDegrees(dob));
			int length = 1;
			if( detectionsPoint5.w[2] < 0.60){
				switch(facing){
					case UP: map.addObstacle(xCord ,yCord -length );break;
					case DOWN: map.addObstacle(xCord,yCord + length);break;
					case LEFT: map.addObstacle(xCord - length,yCord);break;
					case RIGHT: map.addObstacle(xCord + length,yCord);break;
				}
			}else{
				switch(facing){
				case UP: map.addFreeSpace(xCord ,yCord -1 );break;
				case DOWN: map.addFreeSpace(xCord,yCord + 1);break;
				case LEFT: map.addFreeSpace(xCord - 1,yCord);break;
				case RIGHT: map.addFreeSpace(xCord + 1,yCord);break;
			}
			}
		}else{
			switch(facing){
				case UP: map.addFreeSpace(xCord ,yCord -1 );break;
				case DOWN: map.addFreeSpace(xCord,yCord + 1);break;
				case LEFT: map.addFreeSpace(xCord - 1,yCord);break;
				case RIGHT: map.addFreeSpace(xCord + 1,yCord);break;
			}
		}
		map.update();
	}

	private void forward(FloatWA oldPositions) {
		FloatWA current = new FloatWA(3);
		vrep.simxGetObjectPosition(clientID,dr20, -1, current, vrep.simx_opmode_streaming);
		try {
			sleep(60);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		vrep.simxGetObjectPosition(clientID,dr20, -1, current, vrep.simx_opmode_streaming);
		xOldCord = xCord;
		yOldCord = yCord;
		if(facing == UP){
			

			yCord--;
			//double roundOff = Math.round(a * 100.0) / 100.0;
			float test =(float) (yPos - 0.480); // (float) (oldPositions.w[1] -0.480);
			

			
			while(current.w[1] > test){
				vrep.simxGetObjectPosition(clientID,dr20, -1, current, vrep.simx_opmode_streaming);
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 3, vrep.simx_opmode_oneshot);
			}
			yPos -= 0.5;
		}else if(facing == DOWN){
			//yOldCord = yCord;
			yCord++;
			float test = (float)((yPos + 0.480));  //(oldPositions.w[1] +0.480);
			while(current.w[1] < test){
				vrep.simxGetObjectPosition(clientID,dr20, -1, current, vrep.simx_opmode_streaming);
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 3, vrep.simx_opmode_oneshot);
			}
			yPos += 0.5;
		}else if(facing == RIGHT){
			//xOldCord = xCord;
			xCord++;
			float test = (float) (xPos - 0.480);//(oldPositions.w[0] -0.480);
			while(current.w[0] > test){
				vrep.simxGetObjectPosition(clientID,dr20, -1, current, vrep.simx_opmode_streaming);
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 3, vrep.simx_opmode_oneshot);
			}
			xPos -= 0.5;
		}else if(facing == LEFT){
			//xOldCord = xCord;
			xCord--;
			float test = (float) (xPos + 0.480);//(oldPositions.w[0] + 0.480);
			System.out.println(current.w[0] + " " + test);
			while(current.w[0] < test){
				//System.out.println(current.w[0] + " " + test);
				vrep.simxGetObjectPosition(clientID,dr20, -1, current, vrep.simx_opmode_streaming);
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 3, vrep.simx_opmode_oneshot);
			}
			xPos += 0.5;
		}
		vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 0,vrep.simx_opmode_oneshot);
		vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 0, vrep.simx_opmode_oneshot);
			
		test = false;
			
		
		
	}

	private void turnAround() { //not needed...
		//System.out.println("back");
		turnRight(); // Or turn left?
		try {
			sleep(1700);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		test  = false;
		
	}

	private void turnRight() {
		//System.out.println("right");
		FloatWA angles = new FloatWA(3);
		vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
		try {
			sleep(50);
		} catch (InterruptedException e1) {
			// TODO Auto-generated catch block
			e1.printStackTrace();
		}
		vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
		Float fl = new Float(angles.w[2]);
        Double dob = new Double(fl.toString());
		Double degree = Math.toDegrees(dob);
		if(facing == RIGHT){
			if(degree < 0){
				degree = -degree;
			}
			while(degree > 100){//100
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) -3, vrep.simx_opmode_oneshot);
				vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
				 fl = new Float(angles.w[2]);
		         dob = new Double(fl.toString());
				 degree = Math.toDegrees(dob);
				 if(degree < 0){
						degree = -degree;
					}
			}
		}else if(facing == DOWN){
			while(degree > 10){
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) -3, vrep.simx_opmode_oneshot);
				vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
				 fl = new Float(angles.w[2]);
		         dob = new Double(fl.toString());
				 degree = Math.toDegrees(dob);
			}
		}else if(facing == LEFT){
			while(degree > -81){
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) -3, vrep.simx_opmode_oneshot);
				vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
				 fl = new Float(angles.w[2]);
		         dob = new Double(fl.toString());
				 degree = Math.toDegrees(dob);
			}
		}else if(facing == UP){
//			}
			while(degree > -170){
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) -3, vrep.simx_opmode_oneshot);
				vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
				 fl = new Float(angles.w[2]);
		         dob = new Double(fl.toString());
				 degree = Math.toDegrees(dob);
				 //System.out.println(degree);
			}
		}
		test = false;
		vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 0,vrep.simx_opmode_oneshot);
		vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 0, vrep.simx_opmode_oneshot);
		
		facing++;
		if(facing == 5){
			facing = 1;
		}
	}

	private void turnLeft() {
		
		FloatWA angles = new FloatWA(3);
		vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
		try {
			sleep(50);
		} catch (InterruptedException e1) {
			// TODO Auto-generated catch block
			e1.printStackTrace();
		}
		vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
		Float fl = new Float(angles.w[2]);
        Double dob = new Double(fl.toString());
		Double degree = Math.toDegrees(dob);
		if(facing == RIGHT){
			if(degree > 0){
				degree = -degree;
			}
			while(degree < -100){//100
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) -3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 3, vrep.simx_opmode_oneshot);
				vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
				 fl = new Float(angles.w[2]);
		         dob = new Double(fl.toString());
				 degree = Math.toDegrees(dob);
				 if(degree > 0){
						degree = -degree;
					}
			}
		}else if(facing == DOWN){
			while(degree < 170){
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) -3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 3, vrep.simx_opmode_oneshot);
				vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
				 fl = new Float(angles.w[2]);
		         dob = new Double(fl.toString());
				 degree = Math.toDegrees(dob);
			}
		}else if(facing == LEFT){
			while(degree < 79){
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) -3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 3, vrep.simx_opmode_oneshot);
				vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
				 fl = new Float(angles.w[2]);
		         dob = new Double(fl.toString());
				 degree = Math.toDegrees(dob);
			}
		}else if(facing == UP){
//			System.out.println("degree " + degree);
			while(degree < -10){
				vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) -3,vrep.simx_opmode_oneshot);
				vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 3, vrep.simx_opmode_oneshot);
				vrep.simxGetObjectOrientation(clientID,dr20,-1,angles,vrep.simx_opmode_streaming);
				 fl = new Float(angles.w[2]);
		         dob = new Double(fl.toString());
				 degree = Math.toDegrees(dob);
				// System.out.println(degree);
			}
		}
//		try {
//			
//			sleep(1050);
//		} catch (InterruptedException e) {
//			e.printStackTrace();
//		}
		test = false;
		vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 0,vrep.simx_opmode_oneshot);
		vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 0, vrep.simx_opmode_oneshot);
		
		facing--;
		if(facing == 0){
			facing = 4;
		}
		
	}
}

